/*********************************************************************************
*            (c) Copyright,Shenzhen Smart Drone UAV Co., Ltd.  
*                        All Rights Reserved  
* 
* File 	 : cmd_line.c
* By  	 : hirsi
* Date	 :2019-08-01 10:23:25
* Other	 : 
*  
**********************************************************************************/
#include "Sys.h"

#define CMD_LINE_FIFO_SIZE					1024
#define CMD_LEN_MAX							20

#define CL_STATUS_READ_AND_EXE_CMD			0
#define CL_STATUS_DELAY						1


typedef struct{    
    INT8U    len; 
	INT8U 	 idx; 	
	
	INT8U    type;	
	INT8U    chn; 	
	INT16U   delay;
}CMD_LINE_CTL_ST;


static FIFO cmdfifo;
static INT8U status;
static CMD_LINE_CTL_ST cmdline;

//
void cmd_line_scan (void)
{
	INT8U lv;

	if (status == CL_STATUS_READ_AND_EXE_CMD){
		if (cmdline.idx == 0){
			if (ReadFIFO(&cmdfifo, &cmdline.len) == false) return;	
			if (ReadFIFO(&cmdfifo, &cmdline.chn) == false) return;	
			cmdline.type = cmdline.chn&CHN_TYPE_MASK;
			cmdline.chn &= CHN_MASK;
			if (ReadFIFO(&cmdfifo, &lv) == false) return;	
			cmdline.delay = lv&PS_DELAY_MASK;
			cmdline.delay *= (CMD_LINE_TIME_UNIT/CMD_LINE_SCAN_INTERVAL);
			lv &= PS_LV_MASK;
			cmdline.idx = 2;			
		}else if (cmdline.idx < cmdline.len){
			if (ReadFIFO(&cmdfifo, &lv) == false) return;	
			cmdline.delay = lv&PS_DELAY_MASK;
			cmdline.delay *= (CMD_LINE_TIME_UNIT/CMD_LINE_SCAN_INTERVAL);
			lv &= PS_LV_MASK;
			cmdline.idx++;
		}else {
			cmdline.idx = 0;
			 return;
		}
		
		if (cmdline.type == CHN_PWM){
			if (lv == PS_LOW){
				setpwm(cmdline.chn, CL_LOW);
			}else if (lv == PS_MID){
				setpwm(cmdline.chn, CL_MID);
			}else if (lv == PS_HIGH){
				setpwm(cmdline.chn, CL_HIGH);
			}
		}else if (cmdline.type == CHN_SBUS){
			if (lv == PS_LOW){				
				channel_in[cmdline.chn] = CL_LOW;
			}else if (lv == PS_MID){
				channel_in[cmdline.chn] = CL_MID;
			}else if (lv == PS_HIGH){
				channel_in[cmdline.chn] = CL_HIGH;
			}
		//}else if (cmdline.type == CHN_USART){

		}else return;

		status = CL_STATUS_DELAY;

	}else if (status == CL_STATUS_DELAY){
		if (cmdline.delay > 0){
			cmdline.delay--;
		}

		if (cmdline.delay == 0){
			if (cmdline.idx == cmdline.len){
				cmdline.idx = 0;
			}
			status = CL_STATUS_READ_AND_EXE_CMD;
		}		
	}
	
}

void cmd_line_add (INT8U *cmd, INT8U len)
{
	//writebusy = 1;
    WriteFIFO(&cmdfifo, len);
    WriteFIFOs(&cmdfifo, cmd, len);
	//writebusy = 0;
}

void cmd_line_init (void)
{
	INT8U *ptr;

    if ((ptr = (INT8U *)mem_malloc(CMD_LINE_FIFO_SIZE)) == NULL){
        return;
    }        
    InitFIFO(&cmdfifo, ptr, CMD_LINE_FIFO_SIZE);   

	//writebusy = 0;
	status = CL_STATUS_READ_AND_EXE_CMD;
	mem_set((INT8U *)&cmdline, 0, sizeof(cmdline));
}

